#include "can_ros1_driver/can_send.hpp"
#include <queue>
// 0 初始化
canControl::canControl() : nh_("~")
{
    // 1 初始化ROS节点
    nh_.param<double>("wheel_base", wheel_base, 1.0);
    nh_.param<bool>("Autoware", Autoware, 1.0);
    nh_.param<std::string>("twistTopic", twistTopic, "/cmd_vel");

    bBreak = false;
    rate = 1.0;

    /* 打开套接字 */
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (0 > sockfd)
    {
        perror("socket error");
        exit(EXIT_FAILURE);
    }

    /* 指定can0设备 */
    ifr = {0};
    can_addr = {0};
    strcpy(ifr.ifr_name, "can0");
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    /* 将can0与套接字进行绑定 */
    if (0 > bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)))
    {
        perror("bind error");
        close(sockfd);
        exit(EXIT_FAILURE);
    }

    /* 设置过滤规则 */
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);

    /* 发送数据格式 */
    frameE2.can_dlc = 8;    // 一次发送8个字节数据
    frameE2.can_id = 0x0E2; // 帧ID为0x0E2,标准帧
    frameE2.data[4] = 0x00;
    frameE2.data[5] = 0x00;
    frameE2.data[6] = 0x00;

    frame469.can_dlc = 8;    // 一次发送8个字节数据
    frame469.can_id = 0x469; // 帧ID为0x469,标准帧
    frame469.data[0] = 0x20;
    frame469.data[1] = 0x00;
    frame469.data[2] = 0x00;
    frame469.data[5] = 0x00;
    frame469.data[6] = 0x00;

    // 创建订阅控制底盘消息的订阅者
    if (Autoware)
        sub_ = nh_.subscribe(twistTopic, 1000, &canControl::velocityCallback_Autoware, this);
    else
        sub_ = nh_.subscribe(twistTopic, 1000, &canControl::velocityCallback, this);

    // 订阅YOLO停障信息
    // sub_yolo = nh_.subscribe("/breakState", 1000, &canControl::yoloCallback, this);
    sub_feed = nh_.subscribe("/feedback_carState", 1000, &canControl::feedCallback, this);

    timer_autoware_ = nh_.createTimer(ros::Duration(0.02), &canControl::velocityCallback_Autoware_Timer, this);

    //vel_data_.resize(6);
}

// 订阅YOLO停障信息
// void canControl::yoloCallback(const ultralytics_ros::breakState::ConstPtr &msg)
// {
//     ultralytics_ros::breakState yolo_msg = *msg;
//     // 红灯够近(通过面积比判断的)
//     bool bBreak_red = (yolo_msg.red_light && yolo_msg.cost_proportion > 0.005);

//     // // 尾插
//     // break_list.push_back(bBreak_red);
//     // if (break_list.size() > 3)
//     //     // 头删
//     //     break_list.pop_front();
//     // // 为了排除突然出现的误识别红灯，前面两帧也要是红灯,即链表里面全是True
//     // for (std::list<bool>::const_iterator it = break_list.begin(); it != break_list.end(); it++)
//     // {
//     //     if (!(*it))
//     //     {
//     //         bBreak_red = false;
//     //         break;
//     //     }
//     // }

//     // 遇到红灯或障碍物距离够近
//     double min_dis = 0.4, max_dis = 1.0;
//     bBreak = false;
//     rate = 1.0;
//     // 障碍物距离够近
//     if (bBreak_red || (yolo_msg.obstacle && yolo_msg.dis_obstacle <= min_dis && yolo_msg.dis_obstacle > 0.1))
//     {
//         bBreak = true;
//         rate = 0.0;
//     }
//     // 障碍物减速（用正弦函数）
//     else if (yolo_msg.obstacle && yolo_msg.dis_obstacle < max_dis && yolo_msg.dis_obstacle > 0.1)
//     {
//         bBreak = false;
//         // x = (x-min_dis)/(2-min_dis)     0<x<1
//         double t = (yolo_msg.dis_obstacle - min_dis) / (max_dis - min_dis);
//         // y = sin[(pi/2 - 0.2)*x + 0.2]     0.2<y<1
//         rate = sin((M_PI - 0.4) * t / 2 + 0.2);
//     }

//     ROS_WARN("--------------------------------------");
//     ROS_WARN("red_light:%d", yolo_msg.red_light);
//     ROS_WARN("rate:%f", yolo_msg.cost_proportion);
//     ROS_WARN("obstacle:%d", yolo_msg.obstacle);
//     ROS_WARN("dis_obstacle:%f", yolo_msg.dis_obstacle);
//     ROS_WARN("bBreak:%d", bBreak);
// }

// 1 下发CAN消息 根据接收到的速度消息生成控制指令并发送
// 其他节点消息的回调函数
void canControl::velocityCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    double vel_x_cmd = 0.0;
    linear_x_ = msg->linear.x;
    vel_count_++;
    
    vel_data_.push_back(linear_x_);
    if(vel_data_.size() == 5){
        vel_x_cmd = vel_data_[0] + vel_data_[1] + vel_data_[2] + vel_data_[3] + vel_data_[4];
        vel_x_cmd = vel_x_cmd/5.0;
        vel_data_.pop_front();
        linear_x_ = vel_x_cmd;
    }
    if(linear_x_ >= 0)
        angular_z_ = msg->angular.z;
    else
        angular_z_ = -msg->angular.z;

}
// Autoware发送的
void canControl::velocityCallback_Autoware(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    linear_x_ = msg->twist.linear.x;
    angular_z_ = msg->twist.angular.z;
}

void canControl::feedCallback(const can_ros1_driver::CarState::ConstPtr &msg)
{
    driving_mode_ = msg->driving_mode;
    gear_ = msg->gear;
}

void canControl::velocityCallback_Autoware_Timer(const ros::TimerEvent &event)
{
    publishCmd(linear_x_, angular_z_);
}
// 1.1 向底盘发送控制指令
void canControl::publishCmd(float linear_vel, float angular_vel)
{
    if(vel_count_ == last_vel_count_){
        no_vel_cmd_count_++;
    }
    else{
        no_vel_cmd_count_ = 0;
    }
    if(no_vel_cmd_count_ > 5){
        no_vel_cmd_count_ = 10;
        linear_vel = 0.0;
    }

    // std::cout << "linear_vel: " << linear_vel << std::endl;
    last_vel_count_ = vel_count_;
    linear_vel = rate * linear_vel;
    // 根据线速度和角速度生成控制指令的逻辑,将 linear_vel、angular_vel 转换为控制指令
    // 1. 计算挡位Gear
    int gear = 0; // 默认值
    if (linear_vel > 0)
    {
        gear = 1; // 前进挡
    }
    else if (linear_vel < 0)
    {
        gear = 2; // 倒挡
    }
    // 2. 线控驾驶标志位，通过CAN线控
    int line_control = 1;
    // 3. 驻车P标志位
    int parking_brake = 0;
    // 速度为0或速度反向的时候刹车, 探测到红绿灯和障碍物时也要停
    static int last_gear = 0;
    if (bBreak || linear_vel == 0.0 || last_gear + gear == 3)
        parking_brake = 1;
    last_gear = gear;
    // 4. 循环计数，默认为0
    int loop_count = 0;
    uint8_t gear_contrl = static_cast<uint8_t>(loop_count + parking_brake * 8 + line_control * 4 + gear);

    // 5 计算速度byte
    if (linear_vel > 5)
    {
        ROS_WARN("Speed is too fast!");
        linear_vel = 5;
    }
    // printf("Send档位值为:%02x \n", gear_contrl);
    // std::cout << "Send速度值为: " << linear_vel << std::endl;
    uint8_t Speed_L = 0;
    uint8_t Speed_H = 0;
    if (parking_brake != 1)
    {
        Speed_L = static_cast<uint8_t>((int)fabs(linear_vel * 1000) % 256);
        Speed_H = static_cast<uint8_t>((int)fabs(linear_vel * 1000) / 256);
    }

    // 6. 计算低压电器开关和速度校验和
    // 刹车灯
    int brakeLight = 0;
    if (parking_brake)
        brakeLight = 1;
    // 前大灯
    int headlight = 0;
    // 转向灯
    int trafficIndicatorL = 0;
    int trafficIndicatorR = 0;
    if (angular_vel > 0.2)
        trafficIndicatorL = 1;
    else if (angular_vel < -0.2)
        trafficIndicatorR = 1;
    uint8_t LowVoltageEle = static_cast<uint8_t>(brakeLight + headlight * 2 + trafficIndicatorL * 4 + trafficIndicatorR * 8);

    uint8_t checksumSpeed = gear_contrl + Speed_L + Speed_H + LowVoltageEle;

    if (driving_mode_ == 0 || gear_ == 0)
    {
        /* 发送数据 进入线控 协议需要每次先要发速度清空*/
        frameE2.data[0] = gear_contrl;
        frameE2.data[1] = 0x00;
        frameE2.data[2] = 0x00;
        frameE2.data[3] = LowVoltageEle;
        frameE2.data[7] = checksumSpeed;
        ros::Duration(0.01).sleep();
        ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据
    }
    /* if (gear_ == 0)
    {
        /* 发送数据 进入线控 协议需要每次先要发速度清空
        frameE2.data[0] = gear_contrl;
        frameE2.data[1] = 0x00;
        frameE2.data[2] = 0x00;
        frameE2.data[3] = LowVoltageEle;
        frameE2.data[7] = checksumSpeed;
        ros::Duration(0.02).sleep();
        ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

	frame469.data[3] = 0;
	frame469.data[4] = 0;
	frame469.data[7] = 0x20;
	ret = write(sockfd, &frame469, sizeof(frame469)); // 发送数据
	


    }*/
    // 7. 生成速度控制指令 发送数据
    frameE2.data[0] = gear_contrl;
    frameE2.data[1] = Speed_L;
    frameE2.data[2] = Speed_H;
    frameE2.data[3] = LowVoltageEle;
    frameE2.data[7] = checksumSpeed;
    ros::Duration(0.01).sleep();
    ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

    // 8. 计算角度byte
    // 转弯的角度 = arctan(( 角速度 / 线速度 ) * 轴距 )
    // 左转是rZ正方向，而这个底盘最左是负，因此加-号
    double angular;
    if(gear_ != 0){
    angular = -atan((angular_vel / fabs(linear_vel)) * wheel_base);
    }
    else 
	angular = -atan((angular_vel / 0.5) * wheel_base);
    int angular_byte = 10 * angular * 180 / M_PI;
    // int angular_byte = 3.6 * 12 * angular * 180 / M_PI;
    // std::cout << "Send打角值为: " << angular_byte << std::endl;
    uint8_t angular_L = static_cast<uint8_t>((int)(angular_byte + 1024) % 256);
    uint8_t angular_H = static_cast<uint8_t>((int)(angular_byte + 1024) / 256);

    // 9. 计算角度校验和
    uint8_t checksumAng = 0x20 + angular_H + angular_L;

    // 10. 生成角度控制指令 发送数据
    frame469.data[3] = angular_H;
    frame469.data[4] = angular_L;
    frame469.data[7] = checksumSpeed;
    ret = write(sockfd, &frame469, sizeof(frame469)); // 发送数据


}

// 2 析构函数
canControl::~canControl()
{
    /* 发送数据 退出线控 */
    for (int i = 0; i < 8; i++)
        frameE2.data[i] = 0x00;
    ret = write(sockfd, &frameE2, sizeof(frameE2)); // 发送数据

    /* 关闭套接字 */
    close(sockfd);
    exit(EXIT_SUCCESS);
}
